/*! \file  
	\brief Contains functions for pixel conversion and thresholding
 
*/

/*Author: Martin Klein
* Description: Color Thresholding is the header file for performing color 
* 	thresholding using the ColorThreshold class and the PolarCoordinate class
*/

#ifndef COLORTHRESHOLDING_H
#define COLORTHRESHOLDING_H

#ifdef __cplusplus
extern "C" {
#endif

#include "PolarCoordinate.h"
#include "ColorThreshold.h"
#include <math.h>

// These consts are the coefficients for the curve fit which approximates the
// conversion from pixel position to centimeters 
// NOTE: They are calibrated for each robot ROBOT101-ROBOT108
// To make so a robot sees the correct coefficients simply put -DROBOT### (where ### is the 3 digit robot number)
// as a compiler flag during compulation (Unfortunately each robot must be built separately). 
#ifdef ROBOT101
// Curve fit values:
const double X0 = -35.1693;
const double Y0 = -1.58273;
// Curve fit values:
const double X1 = 0.579054;
const double Y1 = 0.0563615;
// Curve fit values:
const double X2 = -0.00357411;
const double Y2 = -0.000376279;
// Curve fit values:
const double X3 = 9.13368e-06;
const double Y3 = 6.98097e-07;

#endif
#ifdef ROBOT102
// Curve fit values:
const double X0 = -35.1693;
const double Y0 = -1.58273;
// Curve fit values:
const double X1 = 0.579054;
const double Y1 = 0.0563615;
// Curve fit values:
const double X2 = -0.00357411;
const double Y2 = -0.000376279;
// Curve fit values:
const double X3 = 9.13368e-06;
const double Y3 = 6.98097e-07;


#endif //ROBOT102
#ifdef ROBOT103
// Curve fit values:
const double X0 = -35.1693;
const double Y0 = -1.58273;
// Curve fit values:
const double X1 = 0.579054;
const double Y1 = 0.0563615;
// Curve fit values:
const double X2 = -0.00357411;
const double Y2 = -0.000376279;
// Curve fit values:
const double X3 = 9.13368e-06;
const double Y3 = 6.98097e-07;


#endif //ROBOT103
#ifdef ROBOT104
// Curve fit values:
const double X0 = -35.1693;
const double Y0 = -1.58273;
// Curve fit values:
const double X1 = 0.579054;
const double Y1 = 0.0563615;
// Curve fit values:
const double X2 = -0.00357411;
const double Y2 = -0.000376279;
// Curve fit values:
const double X3 = 9.13368e-06;
const double Y3 = 6.98097e-07;


#endif //ROBOT104
#ifdef ROBOT105
// Curve fit values:
const double X0 = -35.1693;
const double Y0 = -1.58273;
// Curve fit values:
const double X1 = 0.579054;
const double Y1 = 0.0563615;
// Curve fit values:
const double X2 = -0.00357411;
const double Y2 = -0.000376279;
// Curve fit values:
const double X3 = 9.13368e-06;
const double Y3 = 6.98097e-07;


#endif //ROBOT105
#ifdef ROBOT106
// Curve fit values:
const double X0 = -35.1693;
const double Y0 = -1.58273;
// Curve fit values:
const double X1 = 0.579054;
const double Y1 = 0.0563615;
// Curve fit values:
const double X2 = -0.00357411;
const double Y2 = -0.000376279;
// Curve fit values:
const double X3 = 9.13368e-06;
const double Y3 = 6.98097e-07;

#endif //ROBOT106
#ifdef ROBOT107
// Curve fit values:
const double X0 = -35.1693;
const double Y0 = -1.58273;
// Curve fit values:
const double X1 = 0.579054;
const double Y1 = 0.0563615;
// Curve fit values:
const double X2 = -0.00357411;
const double Y2 = -0.000376279;
// Curve fit values:
const double X3 = 9.13368e-06;
const double Y3 = 6.98097e-07;


#endif //ROBOT107
#ifdef ROBOT108
// Curve fit values:
const double X0 = -35.1693;
const double Y0 = -1.58273;
// Curve fit values:
const double X1 = 0.579054;
const double Y1 = 0.0563615;
// Curve fit values:
const double X2 = -0.00357411;
const double Y2 = -0.000376279;
// Curve fit values:
const double X3 = 9.13368e-06;
const double Y3 = 6.98097e-07;

#endif //ROBOT108

/*! \var const double ERROR
    \brief PolarCoordinate::radius_cm will equal ERROR if there was an error
*/
const double ERROR 	= -1.0;

/*! \var const int ERROR_UCHAR
    \brief unsigned char will equal ERROR_UCHAR if there was an error
*/
const int ERROR_UCHAR 	= 255;

/*! \brief NULL_CT defines NULL for C which does not have NULL predefined
*/
#define NULL_CT 0
/// Macro to convert from row pixel location to row in centimeters
#define CONVERTROW2CM(y) ((y)*(y)*(y)*Y3 + (y)*(y)*Y2 + (y)*Y1 + Y0)

/// Macro to convert column pixel location to column in centimeters
#define CONVERTCOL2CM(x) ((x)*(x)*(x)*X3 + (x)*(x)*X2 + (x)*X1 + X0)


/// Typedef to for ease of use where l stands for local
typedef unsigned char uchar_l;

/*! \brief Converts pixels to polar coordinates
  
   If radius_cm < 0 an error has occurred.
   This function utilizes the macros CONVERTROW2CM and CONVERTCOL2CM.
   First it uses a third order polynomial regression curve fit to 
   convert from pixel location to location in centimeters. Then it uses
   trigonometry to convert from rows and columns to PolarCoordinate. 
   
   \return { Estimated PolarCoordinate if ERROR then it is likely that one 
   of the parameters was null}
*/
PolarCoordinate convertPixelLoc2Polar(
	double* x, double* y, int* sizeX, int* sizeY);

/*! \deprecated Finder uses GPU rather than CPU to perform thresholding
\brief Thresholds images based on ColorThresholds
 

*/
unsigned char pixelThreshold(
	ColorThreshold* thresh, uchar_l* pixelRed, uchar_l* pixelGreen, uchar_l* pixelBlue);



#ifdef __cplusplus
}
#endif
#endif //COLORTHRESHOLDING_H

